﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using KSASupport;
using System.Diagnostics;

using Kinect.Toolbox.Voice;
using System.Windows;

namespace KSATouch
{
    /// <summary>
    /// Main Class responsible for tracking contacts and handing up touch events
    /// </summary>
    public partial class KSATouch
    {
        private const double CombineFingerDistance = 0.3;
        private const double SmoothingCoefficientValue = 0.3;
        // The following hacks are touch offsets, and will eventually need to be included in the calibration routine.
        // in short, a touch from the 'opposite' side will be off by a knuckle length till this is fixed.
        private const int TOPOFFSET = 15;
        private const int BOTTOMOFFSET = 15;

        private InputFilter inputFilter;
        private KSAEngine ksaEngine;

        private DepthManager    depthManager;
        private BlobTracker     blobTracker;
        private BlobTracker     hoveringBlobTracker;
        private BlobAnalyzer    blobAnalyzer;

        private VoiceCommander voiceCommander;



        #region ContactLists
        private List<Contact> currentContacts;

        /// <summary>
        /// Current touch contacts which are being tracked. includes 'down' and 'moved'
        /// </summary>
        public List<Contact> CurrentContacts
        {
            get { return currentContacts; }
        }



        private List<Contact> currentHoveringContacts;
        /// <summary>
        /// Represent contacts which are 'above' the surface of the screen
        /// </summary>
        public List<Contact> CurrentHoveringContacts
        {
            get { return currentHoveringContacts; }
            set { currentHoveringContacts = value; }
        }
        
                    
        #endregion
        Dictionary<int, DepthManager> depthManagers;
        /// <summary>
        /// Collection of depth managers. Each depth manager maps to a Kinect
        /// </summary>
        public Dictionary<int, DepthManager> DepthManagers
        {
            get { return depthManagers; }
            set { depthManagers = value; }
        }
        
        /// <summary>
        /// Instance of a single depth manager for the single Kinect scenario
        /// </summary>
        public DepthManager DepthManager
        {
            get { return depthManager; }
            set { depthManager = value; }
        }

        private double smoothingCoefficient;
        /// <summary>
        /// A value from 0 (high) to 1 (low) which specifies how much smoothing to apply.  Higher smoothing reduces jitter
        /// at the cost of latency
        /// </summary>
        public double SmoothingCoefficient
        {
            get { return smoothingCoefficient; }
            set { smoothingCoefficient = value; }
        }
        

        /// <summary>
        /// Tracks which valid contacts the system is currently aware of
        /// </summary>
        private List<Contact> trackedContacts;
        private List<Contact> trackedHoveringContacts;

        /// <summary>
        /// Responsible For handing up Touch Events to higher level consumers
        /// </summary>
        public KSATouch() : this(1)
        {}

        /// <summary>
        /// Constructor for working with multiple Kinect Sensors
        /// </summary>
        /// <param name="Kinects"></param>
        public KSATouch(int Kinects)
        { 
            ksaEngine = new KSAEngine();
            
            depthManagers = new Dictionary<int, DepthManager>();
            for(int i = 0; i < Kinects; i++)
            {
                // for now, add voice to just the first kinect found
                // TODO: Evalualate how to best handle mutltiple kinects in the future
                ksaEngine.AddKinect(i);
                if (i == 0)
                {
                    voiceCommander = new VoiceCommander(ksaEngine.DepthManagers[0].KinectSensor);
                }
                depthManagers.Add(i,ksaEngine.DepthManagers[i]);
            }
            // Note: When using multiple Kinects, Depthmanagers are initialized automatically
            
            
            // Start listening for blobs
            blobTracker             = new BlobTracker();
            hoveringBlobTracker     = new BlobTracker();
            blobAnalyzer            = new BlobAnalyzer();
            trackedContacts         = new List<Contact>();
            trackedHoveringContacts = new List<Contact>();

            // Jitter filtering settings
            inputFilter = InputFilter.GetInstance();
            // TODO: Parametize this into the depthmanager
            SmoothingCoefficient = SmoothingCoefficientValue;

            foreach (DepthManager manager in ksaEngine.DepthManagers)
            {
                manager.FrameReceived += new KSASupport.DepthManager.FrameReceivedEventHandler(depthManager_FrameReceived);
            }
            try
            {
                voiceCommander.Start();
                voiceCommander.OrderDetected += voiceCommander_OrderDetected;
            }
            catch (Exception exc)
            {
                System.Diagnostics.Debug.WriteLine("Voice commands did not start successfully! " + exc.Message);
            }
        }
        /// <summary>
        /// Event fired when a registered voice command is detected
        /// </summary>
        /// <param name="voiceCommand"></param>
        void voiceCommander_OrderDetected(string voiceCommand)
        {
            OnOrderRecieved(voiceCommand);
        }

        void depthManager_FrameReceived(object Sender, DepthManager.FrameReceivedEventArgs e)
        {
            // First, filter existing blobs.
            // For hovering blobs, join any bounding boxes which intersect into single blobs
            // For other blobs, find ones next to the bounding box of hovering blobs, and filter anything besides those out
            // if we want to detect objects, use anything that is not next to a bounding box from a hovering blob
            List<Contact> currentHoverContacts = null;

            depthManager = (DepthManager)Sender as DepthManager;
            blobTracker.depthManager = depthManager;
            hoveringBlobTracker.depthManager = depthManager;
            // Allow this to be turned on/off in the future
            if (true)
            {
                
                // Note: This modifies blobs in all parts of the depth layer currently.           
                depthManager.HoveringBlobs = blobAnalyzer.ProcessHoverLayer(depthManager.HoveringBlobs);
                depthManager.DepthBlobs = blobAnalyzer.JoinNearBlobs(depthManager.DepthBlobs, CombineFingerDistance);   
                
                currentHoverContacts = blobAnalyzer.ProcessDepthFrame(ref depthManager);
                
            }
            // Go throgh the currentBlobs and extract contacts
                       

            // Step 1: For each blob, find the closest blob, and assign that (DepthBlob.ClosestBlob)
            List<DepthBlob> matchedBlobs = blobTracker.MatchBlobsToClosest(e.CurrentBlobs);             

            // Now find out what is 'moved', 'new' and 'removed'
            List<Contact> currentFrameContacts = new List<Contact>();
            List<Contact> MovedContacts = blobTracker.ProcessContactsForMovedContacts(ref currentFrameContacts, matchedBlobs); // non-debounce tracking
            List<Contact> NewContacts = blobTracker.ProcessContactsForNewContacts(ref currentFrameContacts, matchedBlobs);
            List<Contact> RemovedContacts = blobTracker.ProcessContactsForRemovedContacts(ref currentFrameContacts, ref trackedContacts, matchedBlobs);     


            // repeat for hovering blobs
            blobTracker.UpdateLastFrameBlobs(matchedBlobs);

            // track hovering contacts
            List<DepthBlob> matchedHoveringBlobs = hoveringBlobTracker.MatchBlobsToClosest(e.HoveringBlobs);
            List<Contact> HoveringContacts = new List<Contact>();
            List<Contact> HoveringMovedContacts = hoveringBlobTracker.ProcessContactsForMovedContacts(ref HoveringContacts, matchedHoveringBlobs);
            List<Contact> HoveringNewContacts = hoveringBlobTracker.ProcessContactsForNewContacts(ref HoveringContacts, matchedHoveringBlobs);
            List<Contact> HoveringRemovedContacts = hoveringBlobTracker.ProcessContactsForRemovedContacts(ref HoveringContacts, ref trackedHoveringContacts, matchedHoveringBlobs);
            
            hoveringBlobTracker.UpdateLastFrameBlobs(matchedHoveringBlobs);


            // integrity check..if a moved contact is there, but we've not added it to tracked..then we need to convert it to a contact down. We must only have one
            // add event for each removed event, and vice versa.
            bool cleanupNeeded = false;
            foreach (Contact contact in MovedContacts)
            {
                if (trackedContacts.Where(con => con.Id == contact.Id).SingleOrDefault() == null)
                {
                    contact.ContactState = ContactStates.ContactDown;
                    cleanupNeeded = true;
                }
            }

            if (cleanupNeeded)
            {
                List<Contact> updatedContacts = MovedContacts.Where(con => con.ContactState == ContactStates.ContactDown).ToList();
                NewContacts.AddRange(updatedContacts);
                MovedContacts.RemoveAll(con => con.ContactState == ContactStates.ContactDown);
            }

            // add new contacts to ones which we're aware of
            trackedContacts.AddRange(NewContacts); 
            trackedHoveringContacts.AddRange(HoveringNewContacts);

            // Update any 'touch' contacts here
            foreach (Contact contact in currentFrameContacts)
            {
                // If the contact is on the 'other side' of the table/device, then flip the centerpoint
                if (contact.Side == KSASupport.BlobTracking.BlobSide.Top)
                {
                    contact.CenterPoint = new Point(contact.CenterPoint.X, contact.CenterPoint.Y - TOPOFFSET); // TODO: Parameterize
                }
                if (contact.Side == KSASupport.BlobTracking.BlobSide.Bottom)
                {
                    contact.CenterPoint = new Point(contact.CenterPoint.X, contact.CenterPoint.Y + BOTTOMOFFSET); // TODO: Parameterize
                }
            }

            // Use a filter to reduce jitter            
            foreach (Contact contact in currentFrameContacts)
            {
                if (contact.ContactBlob == null)
                    continue;

                string UniqueContactNameBase = "ContactID" + contact.Id;
                // TODO: Send this up to a depthmanager Setting (possibly)

                inputFilter.InputCoefficient = SmoothingCoefficient;
                double X = inputFilter.GetFilteredValue(UniqueContactNameBase + "X", contact.CenterPoint.X);
                double Y = inputFilter.GetFilteredValue(UniqueContactNameBase + "Y", contact.CenterPoint.Y);

                contact.CenterPoint = new System.Windows.Point(X, Y);
                System.Diagnostics.Debug.WriteLine("Point is at: " + contact.CenterPoint);
            }
            


            // Contact down
            foreach (Contact contact in NewContacts)
            {
                OnContactDown(new ContactEventArgs(contact));
            }

            // Contact Removal event
            foreach(Contact contact in RemovedContacts)
            {
                OnContactUp(new ContactEventArgs(contact));
            }

            // contact changed/moved events
            foreach (Contact contact in MovedContacts)
            { 
                OnContactChanged(new ContactEventArgs(contact));
            }


            // Update any 'touch' contacts here
            foreach (Contact contact in currentFrameContacts)
            {
                contact.DepthManager = DepthManager;
            }
            // update any hover contacts here
            foreach (Contact contact in HoveringContacts)
            {
                contact.DepthManager = DepthManager;
            }

            currentContacts = currentFrameContacts;
            // TODO: Rename these
            //currentHoveringContacts = currentHoverContacts;
            currentHoveringContacts = HoveringContacts;
            OnFrameReady(new FrameReadyEventArgs());

        }
        #region VoiceCommandProcessing

        /// <summary>
        /// Simple way for higher level libraries to add voice commands
        /// </summary>
        /// <param name="voiceCommands"></param>
        public void AddVoiceCommand(string[] voiceCommands)
        {
            voiceCommander.AddOrders(voiceCommands);
        }
        
        #endregion

    }
}
